import time

import grpc
from grpc import insecure_channel
from dianrong_src.proto import rcx340_pb2, rcx340_pb2_grpc

import cv2
import numpy as np


def stream(move_cmd, channel_n_port):
    with insecure_channel(channel_n_port) as channel:

        time1 = time.time()
        stub = rcx340_pb2_grpc.RobotControllerStub(channel)
        requests = rcx340_pb2.MoveRequest(position=move_cmd)

        try:
            response = stub.RobotMoveCommand(requests)
        except:
            print('error!')

        time2 = time.time()

        print(time2 - time1)

move_mode = 'L' # PTP 点到点
                # L 线性插补
                # C 圆弧插补
p1 = 200
p2 = 452
p3 = 0
p4 = 0
p5 = 0
p6 = 0
f1 = 1
f2 = 0
f3 = 0
S = 100

# init_P = "0 250 0 0.0 0.0 1 0 0"
# cmd = "@P20 = " + init_P
# cmd = "@P21 =  -124 382 0 0.0 0.0 1 0 0"
# cmd = "@P22 =  -124 382 88 0.0 0.0 1 0 0"
port = "localhost:50051"
# cmd = "@MOVE L, -100 452 0 0 0.0 0.0 1 0 0,S=50"
cmd = "@MOVE L, P20, P21, P22, P21, P20, DS=10"
# cmd = "@P20 = 0 250 0 0.0 0.0 1 0 0"
# cmd = "@MOVE L, -124 382 88 0.0 0.0 1 0 0, DS = 20"
# cmd = "@MOVE L, P4, P5, DS=10"
stream(cmd, channel_n_port=port)
